/*
 * helpers.cpp
 *
 *  Created on: Oct 14, 2011
 *      Author: andrew
 */

#include "helpers.h"
#include <fstream>


string matToString(Mat mat)
{
	stringstream s;
	for(int j = 0; j < mat.rows; j ++)
	{
		for(int i = 0; i < mat.cols; i ++)
		{
			//s.setf( ios_base::fixed, ios_base::floatfield );
			//s.precision( 2 );
			s.setf(ios_base::scientific);
			s << mat.at<double>(j,i) << '\t';
		}
		s << endl;
	}
	return s.str();
}

Point3d* read3d(string fileName)
{
	Point3d* points = new Point3d[20];
	string line;
	double x, y, z;
	string xStr, yStr, zStr;
	ifstream infile;
	infile.open (fileName.c_str());
	int i = 0;
	while(!infile.eof()) // To get you all the lines.
	{
		getline(infile,line); // Saves the line in STRING.
		xStr = line.substr(0, 7);
		yStr = line.substr(8, 15);
		zStr = line.substr(16, 22);
		istringstream(xStr) >> x;
		istringstream(yStr) >> y;
		istringstream(zStr) >> z;
		points[i].x = x;
		points[i].y = y;
		points[i].z = z;
		i ++;
		x = 0;
		y = 0;
		z = 0;
	}
	infile.close();
	return points;
}

Point3d* read3dNorm(string fileName)
{
	Point3d* points = new Point3d[20];
	string line;
	double x, y, z;
	string xStr, yStr, zStr;
	ifstream infile;
	infile.open (fileName.c_str());
	int i = 0;
	while(!infile.eof()) // To get you all the lines.
	{
		getline(infile,line); // Saves the line in STRING.
		xStr = line.substr(0, 10);
		yStr = line.substr(10, 20);
		zStr = line.substr(20, 30);
		istringstream(xStr) >> x;
		istringstream(yStr) >> y;
		istringstream(zStr) >> z;
		points[i].x = x;
		points[i].y = y;
		points[i].z = z;
		i ++;
		x = 0;
		y = 0;
		z = 0;
	}
	infile.close();
	return points;
}

Point2d* read2d(string fileName)
{
	Point2d* points = new Point2d[20];
	string line;
	double x, y;
	string xStr, yStr;
	ifstream infile;
	infile.open (fileName.c_str());
	int i = 0;
	while(!infile.eof()) // To get you all the lines.
	{
		getline(infile,line); // Saves the line in STRING.
		xStr = line.substr(0, 4);
		yStr = line.substr(4, 8);
		istringstream(xStr) >> x;
		istringstream(yStr) >> y;
		points[i].x = x;
		points[i].y = y;
		i ++;
		x = 0;
		y = 0;
	}
	infile.close();
	return points;
}

Point2d* read2dNorm(string fileName)
{
	Point2d* points = new Point2d[20];
	string line;
	double x, y;
	string xStr, yStr;
	ifstream infile;
	infile.open (fileName.c_str());
	int i = 0;
	while(!infile.eof()) // To get you all the lines.
	{
		getline(infile,line); // Saves the line in STRING.
		xStr = line.substr(0, 10);
		yStr = line.substr(10, 20);
		istringstream(xStr) >> x;
		istringstream(yStr) >> y;
		points[i].x = x;
		points[i].y = y;
		i ++;
		x = 0;
		y = 0;
	}
	infile.close();
	return points;
}

Mat load3dMat(string fileName, int norm)
{
	Point3d* points;
	if(norm == 0)
		points = read3d(fileName);
	else
		points = read3dNorm(fileName);
	Mat mat(20, 4, CV_64F);
	for(int i = 0; i < 20; i++)
	{
		mat.at<double>(i, 0) = points[i].x;
		mat.at<double>(i, 1) = points[i].y;
		mat.at<double>(i, 2) = points[i].z;
		mat.at<double>(i, 3) = 1.0;
	}
	delete points;
	transpose(mat, mat);
	return mat;
}

Mat load2dMat(string fileName, int norm)
{
	Point2d* points;
	if(norm == 0)
		points = read2d(fileName);
	else
		points = read2dNorm(fileName);
	Mat mat(20, 3, CV_64F);
	for(int i = 0; i < 20; i++)
	{
		mat.at<double>(i, 0) = points[i].x;
		mat.at<double>(i, 1) = points[i].y;
		mat.at<double>(i, 2) = 1.0;
	}
	delete points;
	transpose(mat, mat);
	return mat;
}



Mat findTransform(Mat mat)
{
	double* totals = new double[mat.rows - 1];

	//cout << "fdshfkjs" << endl;

	for(int j = 0; j < mat.rows - 1; j++) //each row is a coord: x, y, maybe z, and 1
	{
		totals[j] = 0;
		for(int i = 0; i < mat.cols; i++)
		{
			totals[j] += mat.at<double>(j, i);
		}
	}

	//we now have the totals; we make and apply the transform
	Mat translateMat(mat.rows, mat.rows, CV_64F);
	Mat scaleMat(mat.rows, mat.rows, CV_64F);

	for(int j = 0; j < mat.rows; j++) //each row is a coord: x, y, maybe z, and 1
	{
		for(int i = 0; i < mat.rows; i++)
		{
			translateMat.at<double>(j, i) = 0.0;
			scaleMat.at<double>(j, i) = 0.0;
		}
	}

	for(int i = 0; i < mat.rows; i++)
	{
		translateMat.at<double>(i, i) = 1;
		scaleMat.at<double>(i, i) = 1;
	}
	for(int i = 0; i < mat.rows - 1; i++)
	{
		translateMat.at<double>(i, mat.rows - 1) = -totals[i] / ((double)mat.cols);
	}

	Mat translated;
	gemm(translateMat, mat, 1,  NULL, 0, translated); // add mul


	for(int j = 0; j < translated.rows - 1; j++) //each row is a coord: x, y, maybe z, and 1
	{
		totals[j] = 0;							//btw, we are now using totals as maximums
		for(int i = 0; i < translated.cols; i++)
		{
			if(translated.at<double>(j, i) > totals[j])
				totals[j] = translated.at<double>(j, i);
		}
	}
	//gem(scaleMat, translated, 1,  NULL, 0, translated); // add mul
	for(int i = 0; i < mat.rows - 1; i++)
	{
		scaleMat.at<double>(i, i) = 1.00  / totals[i];
	}

	Mat result;
	gemm(scaleMat, translateMat, 1,  0, 0, result);

	delete totals;
	return result;
}

Mat leastSquares(Mat x, Mat y)
{

	return (((x.t() * x).inv()) * x.t()) * y;
}

void buildLsSolver(Mat pts2d, Mat pts3d, Mat &x, Mat &y) // should have the same num of cols
{
	int entries = pts2d.cols;
	Mat solver(entries * 2, 12, CV_64F);
	for(int i = 0; i < entries; i ++)
	{
		//Build first part
		for(int k = 0; k < 4; k++)
		{
			solver.at<double>(2*i, k) = pts3d.at<double>(k, i);
			solver.at<double>(2*i, k + 4) = 0;
			solver.at<double>(2*i, k + 8) = -pts3d.at<double>(k, i) * pts2d.at<double>(0, i);
		}
		//Build first part
		for(int k = 0; k < 4; k++)
		{
			solver.at<double>(2*i + 1, k) = 0;
			solver.at<double>(2*i + 1, k + 4) = pts3d.at<double>(k, i);
			solver.at<double>(2*i + 1, k + 8) = -pts3d.at<double>(k, i) * pts2d.at<double>(1, i);
		}
	}

	x = solver(Range::all(), Range(0, 11));
	y = solver(Range::all(), Range(11, 12)) * (-1);
}

Mat solveProjection(Mat pts2d, Mat pts3d)
{
	Mat x, y, temp;
	Mat solution(3, 4, CV_64F);
	buildLsSolver(pts2d, pts3d, x, y);
	temp = leastSquares(x, y); // solution as a one-column matrix
	for(int i = 0; i < 11; i++)
	{
		solution.at<double>(i / 4, i % 4) = temp.at<double>(i, 0);
	}
	solution.at<double>(2, 3) = 1;
	return solution;
}

Mat homogeneousToReal(Mat h)
{
	int elements = h.rows;
	Mat real(elements - 1, 1, CV_64F);
	for(int i = 0; i < elements - 1; i++)
	{
		real.at<double>(i, 0) = h.at<double>(i, 0) / h.at<double>(elements - 1, 0);
	}
	return real;
}

double distanceBetween(Mat a, Mat b)
{
	double dist = 0.0;
	for(int j = 0; j < a.rows; j++)
	{
		for(int i = 0; i < a.cols; i++)
		{
			double diff = a.at<double>(j, i) - b.at<double>(j, i);
			dist += diff * diff;
		}
	}
	return sqrt(dist);

}

double residual(Mat a, Mat b) // These are homogeneous points
{
	// first we divide through by last entry to get actual 2d coordinates
	Mat aReal = homogeneousToReal(a);
	Mat bReal = homogeneousToReal(b);

	return distanceBetween(aReal, bReal);
}


void choosePoints(Mat p2d, Mat p3d, int k, int check, Mat& out2d, Mat& out3d, Mat& check2d, Mat& check3d)
{
	out2d = Mat(3, k, CV_64F);
	out3d = Mat(4, k, CV_64F);
	check2d = Mat(3, check, CV_64F);
	check3d = Mat(4, check, CV_64F);
	int totalPts = p2d.cols;
	if(k + check > totalPts) return;

	int* ptsToCopy = new int[k + check]; //don't forget to delete
	for(int i = 0; i < k + check; i++) //populate the random numbers
	{
		int ptIndex = -1;
		bool alreadyChosen = true;
		while(alreadyChosen)
		{
			ptIndex = rand() % totalPts;
			alreadyChosen = false;
			for(int j = 0; j < i; j++)
			{
				alreadyChosen = ((ptIndex==ptsToCopy[j]) || alreadyChosen);
			}
		}
		ptsToCopy[i] = ptIndex;
	}
	//assume that it works
	for(int i = 0; i < k; i++)	//Pull out the ones to use to create an m matrix
	{
		int index = ptsToCopy[i];
		Mat tmp = out2d(Range::all(), Range(i, i + 1));
		p2d(Range::all(), Range(index, index + 1)).copyTo(tmp);

		tmp = out3d(Range::all(), Range(i, i + 1));
		p3d(Range::all(), Range(index, index + 1)).copyTo(tmp);
	}
	for(int i = 0; i < check; i++)	//Pull out the ones to use to create an m matrix
	{
		int index = ptsToCopy[i + k];
		Mat tmp = check2d(Range::all(), Range(i, i + 1));
		p2d(Range::all(), Range(index, index + 1)).copyTo(tmp);

		tmp = check3d(Range::all(), Range(i, i + 1));
		p3d(Range::all(), Range(index, index + 1)).copyTo(tmp);
	}

	delete ptsToCopy;
}

Mat findBestM(Mat pts2d, Mat pts3d, Mat t2, Mat t3)
{
	Mat choices2d, choices3d, check2d, check3d, normalCheck2d, normalCheck3d, t2inv;

	int kVals[3] = {8, 12, 15};
	int checkPoints = 5;
	int residualVals[checkPoints];
	double lowestRes = 1.0e30;
	t2inv = t2.inv();
	Mat bestM(3, 4, CV_64F);

	for(int i = 0; i < 3; i++)
	{
		int k = kVals[i];
		for(int j = 0; j < 10; j ++)
		{
			//1. Randomly choose the points
			choosePoints(pts2d, pts3d, k, checkPoints, choices2d, choices3d, check2d, check3d);

			//2. Normalize the points
			Mat normal2d = t2 * choices2d;
			Mat normal3d = t3 * choices3d;

			//3. Compute projection matrix m
			Mat m = solveProjection(normal2d, normal3d);

			//4. Computes residual for check points
			normalCheck2d = t2 * check2d;
			normalCheck3d = t3 * check3d;
			double residualVal = 0;
			for(int r = 0; r < checkPoints; r++)
			{
				Mat ptReal = normalCheck2d(Range::all(), Range(r, r + 1));
				Mat ptEstimate = m * (normalCheck3d(Range::all(), Range(r, r + 1)));
				ptReal = t2inv * ptReal;
				ptEstimate = t2inv * ptEstimate;
				residualVal += residual(ptReal, ptEstimate);
			}
			residualVal /= 5.0;
			cout << "k = " << k << " Trial " << j + 1 << "\'s average residual: " << residualVal << endl;

			//5. Save the m that gives lowest residual
			if(residualVal < lowestRes)
			{
				bestM = m;
				lowestRes = residualVal;
			}

		}

	}

	cout 	<< endl << "Matrix M yielding lowest residual (" << lowestRes << "):" << endl
			<< matToString(bestM) << endl;
	return bestM;
}

Mat findNormC(Mat m)
{
	Mat q = m(Range(0,3), Range(0,3));
	//cout << matToString(q) << endl;
	Mat m4 = m(Range(0,3), Range(3,4));
	//cout << matToString(m4) << endl;
	Mat c = q.inv() * m4;
	//cout << matToString(q.inv()) << endl;
	//cout << matToString(c) << endl;
	//cout << m4 << endl;
	//cout << m4 << endl;
	//cout << matToString(q.inv()) << endl;
	return c;
}

Mat findCenter(Mat m, Mat t3)
{
	Mat t3inv = t3.inv();
	Mat c_normtmp = findNormC(m);
	Mat c_norm(4, 1, CV_64F);
	c_norm.at<double>(0,0) = c_normtmp.at<double>(0,0);
	c_norm.at<double>(1,0) = c_normtmp.at<double>(1,0);
	c_norm.at<double>(2,0) = c_normtmp.at<double>(2,0);
	c_norm.at<double>(3,0) = 1.0;
	//cout << c_norm.rows << "x" << c_norm.cols << endl;
	Mat result = t3inv * c_norm;
	return result;
}
